#include "rads_slam.hpp"

/**
*    rostopic pub /sradsslam geometry_msgs/Pose "{position: {x: 1.0, y: 2.0, z: 3.0}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}"
*/

// 全局原子变量，用于线程间的通信
std::atomic<bool> should_stop(false);
std::condition_variable cv;
std::mutex mtx;


// 信号处理函数
void signalHandler(int signum) {
    if(SIGINT == signum)
    {
        system("killall -s SIGTERM rads_guard");
        std::this_thread::sleep_for(std::chrono::milliseconds(150));
        ROS_INFO("[Slam] Interrupt signal (%d) received. Shutting down...", signum);
        std::lock_guard<std::mutex> lck(mtx);
        should_stop.store(true);
        cv.notify_all();
        ros::shutdown();
    }
}

int main(int argc, char **argv)
{
    ROS_INFO("[Slam] Starting....");
    ros::init(argc, argv, NODE_SLAM);
    ros::NodeHandle nh;
    // 设置信号处理函数
    signal(SIGINT, signalHandler);
    LidarRtkFusion dataMerger;
    // 订阅rtk和激光雷达(融imu或非)的定位数据
    ros::Subscriber sub_lidar = nh.subscribe("/sradsslam", QUEUE_SIZE, &LidarRtkFusion::lidarCallback,&dataMerger);
    ros::Subscriber sub_rtk = nh.subscribe("/pradsrtk", QUEUE_SIZE,&LidarRtkFusion::rtkCallback,&dataMerger);
    ros::Subscriber sub_ndt = nh.subscribe("/transform_probability", QUEUE_SIZE,&LidarRtkFusion::ndtCallback,&dataMerger);

    // 发布融合之后的数据[10HZ]
    ros::Publisher p = nh.advertise<rads::rads_trackext>("/pradsfusion", QUEUE_SIZE);
    ros::Rate rate(10);
    while (nh.ok() && !should_stop.load())
    {
        p.publish(dataMerger.fusion_data);
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}